#include <errno.h>
#include <unistd.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <linux/i2c-dev.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <linux/types.h>
#include <fcntl.h>
#include "rpi_i2c.h"
#include "rpi_ms5611.h"
#include <math.h>

const double sea_press =1013.25;

ms5611::ms5611(const char *d,const char a,const char r) 
//: 
/* rpi_i2c(d,a,r) */
{

error_str =0;
buf =0;
device_loc=0;
fd =0;

/*bits = 8;
speed = 450000;
delay = 2;
*/
sea_p =sea_press;
if (d) setDevice(d);
if (error_str) return;
if (a) setAddress(a);
if (error()) {
printf("ERROR = %s\n",error());
if (error_str) return;
}
D1 = 0;
D2 = 0;
dT = 0;
TEMP = 0;
OFF = 0; 
SENS = 0; 
P = 0;
pressure_24i=0;
temperature_24i=0;


C = new uint16_t[7];
//write8bit(0x1E);
   // if (error()) printf("ERROR = %s\n",error());
char reset = 0x1E;
if (write(fd,&reset,1) != 1) {
        printf("write reg 8 bit Failed to write to the i2c bus.\n");
    }


usleep(10000); // wait for 10 miliseconds

//printf("Get Eprom DATA\n");
for (int i=0;i<7;i++) {
    //char C8b[] = {0,0};
    usleep(1000); // wait for 10 miliseconds
    
    
    C[i]  = read16bitreg(0xA0 + (i*2));
    if (error()) printf("ERROR = %s\n",error());
    //printf("C[%d] = %d\n",i,C[i]);    
}
/*
C[0] = 40127;
C[1] = 36924;
C[2] = 23317;
C[3] = 23282;
C[4] = 33464;
C[5] = 28312;
*/
/*
for (int l=0;l<6;l++) { 
    C[l] =C[l+1]; 
}
*/

}

ms5611::~ms5611(){
delete [] C;
}


long ms5611::getVal(char r) { 
    long ret =0;
    char zero =0x0;
    
    if (write(fd,&r,1) != 1) {
        printf("write reg 8 bit Failed to write to the i2c bus.\n");
    }
    
    usleep(10000);
    
    if (write(fd,&zero,1) != 1) {
        printf("write reset 8 bit Failed to write to the i2c bus.\n");
    }
    uint8_t d[] = {0,0,0};
    int h =read(fd,&d,3);
    if (h != 3) {
        setError("Failed to read from the i2c bus.");
        printf("Failed to read from the i2c bus %d.\n",h);
        
    }
    
    
    for (int k=0;k<3;k++) {
        //d[k] = read8bitreg(r+k);
        //printf("getVal %d:%d\n",k,d[k]); 
    }
    
    ret = d[0] * (unsigned long)65536 + d[1] * (unsigned long)256 + d[2];
    //ret = (long(d[0]) << 16 ) | (long(d[1]) << 8 ) | (long(d[2])  );
    return ret;
    
 }
void ms5611::acquire() {
    
    D1 =getVal(0x48); //Pressure
    D2 =getVal(0x58); //Temperature
    
    dT   = D2 - ((uint32_t)C[5] << 8);
    
    TEMP = (int64_t)dT * (int64_t)C[6] / 8388608 + 2000;
    
    //printf("Inputs Temp D2:%d,dT:%d,C6:%d,C5%d\n",D2,dT,C[6],C[5]);
    
    
    OFF  = ((int64_t)C[2] << 16) + ((dT * C[4]) >> 7);
    SENS = ((int32_t)C[1] << 15) + ((dT * C[3]) >> 8);

  if(TEMP < 2000) // if temperature lower than 20 Celsius 
  {
        int32_t T1    = 0;
    int64_t OFF1  = 0;
    int64_t SENS1 = 0;

    T1    = pow(dT, 2) / 2147483648;
    OFF1  = 5 * pow((TEMP - 2000), 2) / 2;
    SENS1 = 5 * pow((TEMP - 2000), 2) / 4;
    
    if(TEMP < -1500) // if temperature lower than -15 Celsius 
    {
      OFF1  = OFF1 + 7 * pow((TEMP + 1500), 2); 
      SENS1 = SENS1 + 11 * pow((TEMP + 1500), 2) / 2;
    }
    
    TEMP -= T1;
    OFF -= OFF1; 
    SENS -= SENS1;
  }

    //double pressure_v;
    //double temperture_v;
  temperature_v = (double)TEMP / (double)100; 
  
  P  = ((int64_t)D1 * SENS / 2097152 - OFF) / 32768;

  pressure_v = (double)P / (double)100;
  
  //printf("Temperature:%f\nPressure:%f\n",temperture_v,pressure_v); 
 
}
double ms5611::pressure() { return pressure_v;}
double ms5611::temperature() { return temperature_v;}
 
void ms5611::measureAvg(const int a) {
    double avgPressure=0;
    double avgTemperature =0;
    for (int c=0;c<a;c++) {
        acquire();
        avgPressure += pressure_v;
        avgTemperature += temperature_v;
    }
    pressure_v = avgPressure/double(a);
    temperature_v = avgTemperature/double(a);
    

}
 

double ms5611::calcAltitude() {
 //pressure_v =1006;
  double altitude_v = ((pow((sea_p / pressure_v), 1/5.257) - 1.0) * (temperature_v + 273.15)) / 0.0065;
  return altitude_v;
  
  
}


void ms5611::setSeaLevelPressure(double p) { sea_p =p; }

double ms5611::knownHeight(double h) {

double a = (h  * 0.0065) / (temperature_v + 273.15) + 1.0 ;
 sea_p = exp(log(a) * 5.257)*pressure_v  ;   


  //sea_p = log(h *   0.0065 / (temperature_v + 273.15) + 1.0) / log(1/5.257) * pressure_v;
  //printf("sea level pressure:%f\n",sea_p);
  return sea_p;
}

